#ifdef GUI
#include "display.h"

MapDisplay::MapDisplay(char *wName)
{
	strncpy(name, wName, 32);
	r_x = 0; r_y = 0; r_theta = 0;
	g_x = 0; g_y = 0; g_theta = 0;

	frame = cvCreateImage(cvSize(w_w, w_h), IPL_DEPTH_8U, 3);
}

void MapDisplay::addFeature(double x, double y)
{
	feature f = {x, y};
	features.insert(features.end(), f);
}

void MapDisplay::setGuess(double x, double y, int i)
{
	feature f = {x, y};

	if(i < 0 || i > guesses.size())
		return;

	if(i == guesses.size())
		guesses.insert(guesses.end(), f);
	else
		guesses[i] = f;
}

void MapDisplay::moveRobot(double p, double theta)
{
	r_theta += theta;
	r_x += p*cos(r_theta);
	r_y += p*sin(r_theta);
}

void MapDisplay::guessRobot(double x, double y, double theta)
{
	g_x = x;
	g_y = y;
	g_theta = theta;
}

const IplImage* MapDisplay::renderMap()
{
	cvSet(frame, CV_RGB(255,255,255));

	// figure out the max size of the map.

	double minX = g_x,  minY = g_y;
	double maxX = g_x, maxY = g_y;

	for(int i=0; i < features.size(); i++) {
		double fx = features[i].x;
		double fy = features[i].y;

		if(fx > maxX)
			maxX = fx;
		else if(fx < minX)
			minX = fx;

		if(fy > maxY)
			maxY = fy;
		else if(fy < minY)
			minY = fy;
	}
	for(int i=0; i < guesses.size(); i++) {
		double fx = guesses[i].x;
		double fy = guesses[i].y;

		if(fx > maxX)
			maxX = fx;
		else if(fx < minX)
			minX = fx;

		if(fy > maxY)
			maxY = fy;
		else if(fy < minY)
			minY = fy;
	}

	int const border = 30;

	// scale the map to the proper size
	double scaleX = (w_w-2*border)/(maxX-minX);		// to scale the map
	double scaleY = (w_h-2*border)/(maxY-minY);
	//scaleX = .2;
	//scaleY = .2;

	double mx, my;						// to shift the map
	mx = (minX > 0) ? minX-border/scaleX:-minX+border/scaleX;
	my = (minY > 0) ? minY-border/scaleY:-minY+border/scaleY;
	//mx = 1100;
	//my = 1100;

	// draw robot
	// robot is a triangle which will be represented as set of points
	// that way the points can be easily rotated then translated

	// robot points centered around <0,0>
	CvPoint r_pts[3] = {
		cvPoint(-15, -15),
		cvPoint(-15, 15),
		cvPoint(30, 0),
	};
	CvPoint g_pts[3] = {
		cvPoint(-15, -15),
		cvPoint(-15, 15),
		cvPoint(30, 0),
	};

	// place the points;
	for(int i=0; i < 3; i++) {
		// put point in homogenous coordinates
		Mat pt = (Mat_<double>(3,1) << r_pts[i].x, r_pts[i].y, 1);

		// rotate and translate point
		Mat RT = (Mat_<double>(2,3) <<
			cos(r_theta), -sin(r_theta), (r_x+mx)*scaleX,
			sin(r_theta), cos(r_theta), (r_y+my)*scaleY
		);

		pt = RT*pt;

		r_pts[i].x = (int)pt.at<double>(0);
		r_pts[i].y = (int)pt.at<double>(1);

		// do the same with the guessed robot
		pt = (Mat_<double>(3,1) << g_pts[i].x, g_pts[i].y, 1);

		// rotate and translate point
		RT = (Mat_<double>(2,3) <<
			cos(g_theta), -sin(g_theta), (g_x+mx)*scaleX,
			sin(g_theta), cos(g_theta), (g_y+my)*scaleY
		);

		pt = RT*pt;

		g_pts[i].x = (int)pt.at<double>(0);
		g_pts[i].y = (int)pt.at<double>(1);
	}

	// draw robot
	int npts=3;
	CvPoint *wtfcv_pts[3];
	for(int i=0; i < 3; i++)
		wtfcv_pts[i] = &r_pts[i];

//	cvFillPoly(frame, wtfcv_pts, &npts, 1, CV_RGB(0,255,0));

	// draw features
	for(int i=0; i < features.size(); i++) {
		int fx = (int)((features[i].x+mx)*scaleX);
		int fy = (int)((features[i].y+my)*scaleY);
		cvCircle(frame, cvPoint(fx, fy), 2, CV_RGB(255,0,0));
	}

	// draw robot guess
	for(int i=0; i < 3; i++)
		wtfcv_pts[i] = &g_pts[i];

	cvFillPoly(frame, wtfcv_pts, &npts, 1, CV_RGB(255,0,255));

	// fix coords by flipping around x axis
	cvFlip(frame, frame);

	// draw guesses
	int j=0;
	for(int i=0; i < guesses.size(); i++) {
		int fx = (int)((guesses[i].x+mx)*scaleX);
		int fy = -(int)((guesses[i].y+my)*scaleY) + 600;
		cvCircle(frame, cvPoint(fx, fy), 3, CV_RGB(0,0,255), -1);

		if(guesses.size() > 5) {
			if(!(i%(guesses.size()/6))&&i) {
				CvFont f;
				cvInitFont(&f, CV_FONT_HERSHEY_SIMPLEX, .5,.5);
				char dist[32];
				int lfx = (int)((guesses[i-guesses.size()/6].x+mx)*scaleX);
				int lfy = -(int)((guesses[i-guesses.size()/6].y+my)*scaleY)+600;
				sprintf(dist, "%.0f", sqrt(pow(lfx/scaleX-fx/scaleX,2) + pow(lfy/scaleY-fy/scaleY,2)));

				cvLine(frame, cvPoint(fx,fy), cvPoint(lfx, lfy), CV_RGB(255,0,0));
				cvPutText(frame, dist, cvPoint((fx+lfx)/2, (fy+lfy)/2), &f, cvScalarAll(0));

				if(!(j++%2)) {
					double dgx = (g_x+mx)*scaleX;
					double dgy = -(g_y+my)*scaleY+600;
					sprintf(dist, "%.0f", sqrt(pow((dgx-fx)/scaleX,2) + pow((dgy-fy)/scaleY,2)));

					cvPutText(frame, dist, cvPoint((fx+dgx)/2, (fy+dgy)/2), &f, cvScalarAll(0));
					cvLine(frame, cvPoint(fx,fy), cvPoint(dgx, dgy), CV_RGB(0,255,0));
				}
			}
		}
	}

	// render the frame
	return frame;
}

MapDisplay::~MapDisplay()
{
	cvReleaseImage(&frame);
}

#endif
